﻿#include "Camera/camera.h"


Camera::Camera(QObject *parent)
    : QObject(parent)
{
    //设备初始化为0个相机
    stDeviceList.nDeviceNum = 0;
}


Camera::~Camera(){

    turnOffCamera(CameraName::RobotArm);

    turnOffCamera(CameraName::DataMatrix);
}

//
bool Camera::turnOnCamera(){
    /*
    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));

    // ch:设备枚举 | en:Enum device
    nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
    if (MV_OK != nRet)
    {
        printf("Enum Devices fail! nRet [0x%x]\n", nRet);
        //            break;
    }

    // ch:显示设备信息 | en:Show devices
    if (stDeviceList.nDeviceNum > 0)
    {
        for (unsigned int i = 0; i < stDeviceList.nDeviceNum; i++)
        {
            printf("[device %d]:\n", i);
            MV_CC_DEVICE_INFO* pDeviceInfo = stDeviceList.pDeviceInfo[i];
            if (NULL == pDeviceInfo)
            {
                break;
            }
            PrintDeviceInfo(i, pDeviceInfo);
        }
    }
    else
    {
        printf("Find No Devices!\n");
    }

    // ch:选择相机 | en:Select device
    unsigned int nIndex = 0;
    //printf("Please Input camera index(0-%d):", stDeviceList.nDeviceNum - 1);
    //scanf_s("%d", &nIndex);
    if (nIndex >= stDeviceList.nDeviceNum)
    {
        printf("Input error!\n");
    }

    // ch:创建设备句柄 | en:Create handle
    nRet = MV_CC_CreateHandle(&handle, stDeviceList.pDeviceInfo[nIndex]);
    if (MV_OK != nRet)
    {
        printf("Create Handle fail! nRet [0x%x]\n", nRet);
    }

    // ch:打开设备 | en:Open device
    nRet = MV_CC_OpenDevice(handle);
    if (MV_OK != nRet)
    {
        printf("Open Device fail! nRet [0x%x]\n", nRet);
    }

    //ui->turnOnCamera->setEnabled(false);
    //ui->turnOffCamera->setEnabled(true);

    // ch:探测最佳Packet大小（只支持GigE相机） | en:Detection network optimal package size(It only works for the GigE camera)
    if (MV_GIGE_DEVICE == stDeviceList.pDeviceInfo[nIndex]->nTLayerType)
    {
        int nPacketSize = MV_CC_GetOptimalPacketSize(handle);
        if (nPacketSize > 0)
        {
            // 设置Packet大小
            nRet = MV_CC_SetIntValue(handle, "GevSCPSPacketSize", nPacketSize);
            if (MV_OK != nRet)
            {
                printf("Warning: Set Packet Size fail! nRet [0x%x]!", nRet);
            }
        }
        else
        {
            printf("Warning: Get Packet Size fail! nRet [0x%x]!", nPacketSize);
        }
    }

    // ch:关闭触发模式 | en:Set trigger mode as off
    nRet = MV_CC_SetEnumValue(handle, "TriggerMode", 0);
    if (MV_OK != nRet)
    {
        printf("Set Trigger Mode fail! nRet [0x%x]\n", nRet);
    }

    // ch:获取图像大小 | en:Get payload size
    MVCC_INTVALUE stParam;
    memset(&stParam, 0, sizeof(MVCC_INTVALUE));
    nRet = MV_CC_GetIntValue(handle, "PayloadSize", &stParam);
    if (MV_OK != nRet)
    {
        printf("Get PayloadSize fail! nRet [0x%x]\n", nRet);
    }
    unsigned int nPayloadSize = stParam.nCurValue;

    // ch:初始化图像信息 | en:Init image info
    memset(&stImageInfo, 0, sizeof(MV_FRAME_OUT_INFO_EX));
    pData = (unsigned char *)malloc(sizeof(unsigned char)* (nPayloadSize));
    if (NULL == pData)
    {
        printf("Allocate memory failed.\n");
    }
    memset(pData, 0, sizeof(pData));

    // ch:开始取流 | en:Start grab image
    nRet = MV_CC_StartGrabbing(handle);
    if (MV_OK != nRet)
    {
        printf("Start Grabbing fail! nRet [0x%x]\n", nRet);
    }

    // ch:获取一帧图像，超时时间1000ms | en:Get one frame from camera with timeout=1000ms
    nRet = MV_CC_GetOneFrameTimeout(handle, pData, nPayloadSize, &stImageInfo.stFrameInfo, 1000);
    if (MV_OK == nRet)
    {
        printf("Get One Frame: Width[%d], Height[%d], FrameNum[%d]\n",
               stImageInfo.stFrameInfo.nWidth, stImageInfo.stFrameInfo.nHeight, stImageInfo.stFrameInfo.nFrameNum);
    }
    else
    {
        printf("Get Frame fail! nRet [0x%x]\n", nRet);
    }
    //QMessageBox::information(this, "提示\t", "相机已打开\t");
    
    // Stop grab image
    nRet = MV_CC_StopGrabbing(handle);
    if (MV_OK != nRet)
    {
        printf("Stop Grabbing fail! nRet [0x%x]\n", nRet);
        //break;
    }

    // Close device
    nRet = MV_CC_CloseDevice(handle);
    if (MV_OK != nRet)
    {
        printf("ClosDevice fail! nRet [0x%x]\n", nRet);
        //break;
    }

    // Destroy handle
    nRet = MV_CC_DestroyHandle(handle);
    if (MV_OK != nRet)
    {
        printf("Destroy Handle fail! nRet [0x%x]\n", nRet);
        //break;
    }
    */
    return true;
}
//
bool Camera::turnOffCamera(){
    /*
    // ch:停止取流 | en:Stop grab image
    nRet = MV_CC_StopGrabbing(handle);
    if (MV_OK != nRet)
    {
        printf("Stop Grabbing fail! nRet: [0x%x]\n", nRet);
    }

    // ch:关闭设备 | en:Close device
    nRet = MV_CC_CloseDevice(handle);
    if (MV_OK != nRet)
    {
        printf("CloseDevice fail! nRet: [0x%x]\n", nRet);
    }

    // ch:销毁句柄 | en:Destroy handle
    if (handle)
    {
        MV_CC_DestroyHandle(handle);
        handle = NULL;
    }

    // ch:释放内存 | en:Free memery
    if (pData)
    {
        free(pData);
        pData = NULL;
    }

    //ui->turnOffCamera->setEnabled(false);
    //ui->turnOnCamera->setEnabled(true);
    //QMessageBox::information(this, "提示\t", "相机已关闭\t");
    return true;
}
//
bool Camera::GetImage(cv::Mat &image){

*/
   return true;
}

bool Camera::set_exposure_time(int value){
/*
    if (handle == NULL)
    {
        printf("set_exposure_time fail! nRet: [0x%x]\n", nRet);
        return false;
    }
    else{
        nRet = MV_CC_SetExposureTime(handle, value);
        if (nRet == MV_OK)
        {
           return true;
        }
        else{

           return false;
        }
    }
    */
    return false;
}

bool Camera::get_exposure_time(int& value){
/*
    if (handle == NULL)
    {
        printf("set_exposure_time fail! nRet: [0x%x]\n", nRet);
        return false;
    }
    else{

        MVCC_FLOATVALUE data;
        nRet = MV_CC_GetExposureTime(handle, &data);
        if (nRet == MV_OK)
        {
           value = data.fCurValue;
           return true;
        }
        else{
           return false;
        }
    }
    */

    return false;
}

// print the discovered devices information to user
bool Camera::PrintDeviceInfo(int index , MV_CC_DEVICE_INFO* pstMVDevInfo)
{
    if (NULL == pstMVDevInfo)
    {
        printf("The Pointer of pstMVDevInfo is NULL!\n");
        return false;
    }
    if (pstMVDevInfo->nTLayerType == MV_GIGE_DEVICE)
    {
        int nIp1 = ((pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0xff000000) >> 24);
        int nIp2 = ((pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x00ff0000) >> 16);
        int nIp3 = ((pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x0000ff00) >> 8);
        int nIp4 = (pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp & 0x000000ff);
        
        const int robotarm_camera_ip = 141; //192.168.100.141
        const int datamatrix_camera_ip = 142; //192.168.100.142_ 
        //set different camera to different index

        if(nIp4 == robotarm_camera_ip){
            DeviceHandle.emplace(CameraName::RobotArm, handle_robotarm);
            DeviceIndex.emplace(CameraName::RobotArm, index);
        }
        else if(nIp4 == datamatrix_camera_ip){
            DeviceHandle.emplace(CameraName::DataMatrix,handle_datamatrix);
            DeviceIndex.emplace(CameraName::DataMatrix, index);
        }

        // print current ip and user defined name
        printf("CurrentIp: %d.%d.%d.%d\n" , nIp1, nIp2, nIp3, nIp4);
        printf("UserDefinedName: %s\n\n" , pstMVDevInfo->SpecialInfo.stGigEInfo.chUserDefinedName);
    }
    else if (pstMVDevInfo->nTLayerType == MV_USB_DEVICE)
    {
        printf("UserDefinedName: %s\n", pstMVDevInfo->SpecialInfo.stUsb3VInfo.chUserDefinedName);
        printf("Serial Number: %s\n", pstMVDevInfo->SpecialInfo.stUsb3VInfo.chSerialNumber);
        printf("Device Number: %d\n\n", pstMVDevInfo->SpecialInfo.stUsb3VInfo.nDeviceNumber);
    }
    else
    {
        printf("Not support.\n");
    }

    return true;
}

bool Camera::Initial_cameras(){

    if(stDeviceList.nDeviceNum > 0){
        return true;
    }

    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));

    // ch:设备枚举 | en:Enum device
    nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
    if (MV_OK != nRet)
    {
        printf("Enum Devices fail! nRet [0x%x]\n", nRet);
    }

    // ch:显示设备信息 | en:Show devices
    if (stDeviceList.nDeviceNum > 0)
    {
        for (unsigned int i = 0; i < stDeviceList.nDeviceNum; i++)
        {
            printf("[device %d]:\n", i);
            MV_CC_DEVICE_INFO* pDeviceInfo = stDeviceList.pDeviceInfo[i];
            if (NULL == pDeviceInfo)
            {
                break;
            }
            PrintDeviceInfo(i, pDeviceInfo);
        }
    }
    else
    {
        printf("Find No Devices!\n");
    }
}

bool Camera::isOpen(CameraName camera_name){

    //check
    if(DeviceHandle.find(camera_name) != DeviceHandle.end()){
        if(DeviceHandle.at(camera_name) != NULL){
            return true;
        }
        else{
            return false;
        }
    }
    else{
        return false;
    }



}

//
bool Camera::turnOnCamera(CameraName camera_name){
    
    //check
    if(DeviceHandle.find(camera_name) != DeviceHandle.end()){
        if(DeviceHandle.at(camera_name) != NULL){
            return true;
        }
    }

    //Then try to open camera;
    Initial_cameras();
    int camera_index;

    if(DeviceIndex.find(camera_name) != DeviceIndex.end()){
       camera_index = DeviceIndex.at(camera_name);
    }
    else{
        return false;
    }

    return turnOnDevice(camera_name,
                            stDeviceList.pDeviceInfo[camera_index]);

}

bool Camera::turnOffCamera(CameraName camera_name){
    
    if(DeviceHandle.find(camera_name) == DeviceHandle.end()){
        return false;
    }

    return turnOffDevice(camera_name);

    return true;
}

bool Camera::turnOnDevice(CameraName camera_name, MV_CC_DEVICE_INFO* pstMVDevInfo){

    // 设备不可连接，重新输入
    if (false == MV_CC_IsDeviceAccessible(pstMVDevInfo, MV_ACCESS_Exclusive))
    {
        printf("Can't connect! ");
        return false;
    }

    void * handle = nullptr;
    // ch:创建设备句柄 | en:Create handle
    nRet = MV_CC_CreateHandle(&handle, pstMVDevInfo);
    if (MV_OK != nRet)
    {
        printf("Create Handle fail! nRet [0x%x]\n", nRet);
        return false;
    }


    // ch:打开设备 | en:Open device
    nRet = MV_CC_OpenDevice(handle);
    if (MV_OK != nRet)
    {
        printf("Open Device fail! nRet [0x%x]\n", nRet);
        return false;
    }
    else{
        DeviceHandle.at(camera_name) = handle;
        return true;
    }


    // ch:探测最佳Packet大小（只支持GigE相机） | en:Detection network optimal package size(It only works for the GigE camera)
    if (MV_GIGE_DEVICE == pstMVDevInfo->nTLayerType)
    {
        int nPacketSize = MV_CC_GetOptimalPacketSize(handle);
        if (nPacketSize > 0)
        {
            // 设置Packet大小
            nRet = MV_CC_SetIntValue(handle, "GevSCPSPacketSize", nPacketSize);
            if (MV_OK != nRet)
            {
                printf("Warning: Set Packet Size fail! nRet [0x%x]!", nRet);
                return false;
            }
        }
        else
        {
            printf("Warning: Get Packet Size fail! nRet [0x%x]!", nPacketSize);
            return false;
        }
    }

    // ch:关闭触发模式 | en:Set trigger mode as off
    nRet = MV_CC_SetEnumValue(handle, "TriggerMode", 0);
    if (MV_OK != nRet)
    {
        printf("Set Trigger Mode fail! nRet [0x%x]\n", nRet);
        return false;
    }

}

bool Camera::turnOffDevice(CameraName camera_name){

    auto& handle = DeviceHandle.at(camera_name);
    if( handle == nullptr){
        return false;
    }

    qDebug() << "turnOffDevice " << handle;

    // ch:停止取流 | en:Stop grab image
    //nRet = MV_CC_StopGrabbing(handle);
    //if (MV_OK != nRet)
    //{
    //    printf("Stop Grabbing fail! nRet: [0x%x]\n", nRet);
    //}

    // ch:关闭设备 | en:Close device
    nRet = MV_CC_CloseDevice(handle);
    if (MV_OK != nRet)
    {
        printf("CloseDevice fail! nRet: [0x%x]\n", nRet);
        return false;
    }

    // ch:销毁句柄 | en:Destroy handle
    if (handle)
    {
        nRet = MV_CC_DestroyHandle(handle);
        handle = NULL;
    }


    if(nRet == MV_OK){
        return true;
    }
    else{
        return false;
    }
}

bool Camera::GetImage(CameraName camera_name, cv::Mat & image){
    
    int nRet = MV_OK;
    unsigned char * pData = NULL;

    if(DeviceHandle.find(camera_name) == DeviceHandle.end()){
        return false;
    }

    if(DeviceIndex.find(camera_name) == DeviceIndex.end()){
        return false;
    }

    auto& handle = DeviceHandle.at(camera_name);
    int nIndex = DeviceIndex.at(camera_name);



    // ch:获取图像大小 | en:Get payload size
    MVCC_INTVALUE stParam;
    memset(&stParam, 0, sizeof(MVCC_INTVALUE));
    nRet = MV_CC_GetIntValue(handle, "PayloadSize", &stParam);
    if (MV_OK != nRet)
    {
        printf("Get PayloadSize fail! nRet [0x%x]\n", nRet);
        return false;
    }
    unsigned int nPayloadSize = stParam.nCurValue;

    // ch:初始化图像信息 | en:Init image info
    MV_FRAME_OUT_INFO_EX stImageInfo = { 0 };
    memset(&stImageInfo, 0, sizeof(MV_FRAME_OUT_INFO_EX));
    pData = (unsigned char *)malloc(sizeof(unsigned char)* (nPayloadSize));
    if (NULL == pData)
    {
        printf("Allocate memory failed.\n");
        return false;
    }
    memset(pData, 0, sizeof(pData));

    // ch:开始取流 | en:Start grab image
    nRet = MV_CC_StartGrabbing(handle);
    if (MV_OK != nRet)
    {
        printf("Start Grabbing fail! nRet [0x%x]\n", nRet);
        return false;
    }

    // ch:获取一帧图像，超时时间1000ms | en:Get one frame from camera with timeout=1000ms
    nRet = MV_CC_GetOneFrameTimeout(handle, pData, nPayloadSize, &stImageInfo, 5000);
    if (MV_OK == nRet)
    {
        printf("Get One Frame: Width[%d], Height[%d], FrameNum[%d]\n",
               stImageInfo.nWidth, stImageInfo.nHeight, stImageInfo.nFrameNum);
    }
    else
    {
        printf("Get Frame fail! nRet [0x%x]\n", nRet);
        return false;
    }

    // ch:停止取流 | en:Stop grab image
    nRet = MV_CC_StopGrabbing(handle);
    if (MV_OK != nRet)
    {
        printf("Stop Grabbing fail! nRet [0x%x]\n", nRet);
        return false;
    }

    // ch:关闭设备 | en:Close device
    //nRet = MV_CC_CloseDevice(handle);
    //if (MV_OK != nRet)
    //{
    //    printf("ClosDevice fail! nRet [0x%x]\n", nRet);
    //    break;
    //}


    // ch:数据转换 | en:Convert image data
    bool bConvertRet = false;

    bConvertRet = Convert2Mat(&stImageInfo, pData);


    // ch:释放内存 | en:Free memery
    if (pData)
    {
        free(pData);
        pData = NULL;
    }

    // ch:显示转换结果 | en:Print result
    if (bConvertRet)
    {
        printf("OpenCV format convert finished.\n");
        image = current_Image.clone();
        return true;
    }
    else
    {
        printf("OpenCV format convert failed.\n");
        return false;
    }

}

bool Camera::set_exposure_time(CameraName camera_name, int value){
    
    if(DeviceHandle.find(camera_name) == DeviceHandle.end()){
        return false;
    }

    auto handle = DeviceHandle.at(camera_name);

    if (handle == NULL)
    {
        printf("set_exposure_time fail! nRet: [0x%x]\n", nRet);
        return false;
    }
    else{
        nRet = MV_CC_SetExposureTime(handle, value);
        if (nRet == MV_OK)
        {
           return true;
        }
        else{

           return false;
        }
    }
}

bool Camera::get_exposure_time(CameraName camera_name, int& value){
    
    if(DeviceHandle.find(camera_name) == DeviceHandle.end()){
        return false;
    }

    void * handle = DeviceHandle.at(camera_name);


    if (handle == NULL)
    {
        printf("set_exposure_time fail! nRet: [0x%x]\n", nRet);
        return false;
    }
    else{

        MVCC_FLOATVALUE data;
        nRet = MV_CC_GetExposureTime(handle, &data);
        if (nRet == MV_OK)
        {
           value = data.fCurValue;
           return true;
        }
        else{
           return false;
        }
    }
    return true;
}


int Camera::RGB2BGR( unsigned char* pRgbData, unsigned int nWidth, unsigned int nHeight )
{
    if ( NULL == pRgbData )
    {
        return MV_E_PARAMETER;
    }

    for (unsigned int j = 0; j < nHeight; j++)
    {
        for (unsigned int i = 0; i < nWidth; i++)
        {
            unsigned char red = pRgbData[j * (nWidth * 3) + i * 3];
            pRgbData[j * (nWidth * 3) + i * 3]     = pRgbData[j * (nWidth * 3) + i * 3 + 2];
            pRgbData[j * (nWidth * 3) + i * 3 + 2] = red;
        }
    }

    return MV_OK;
}


// convert data stream in Mat format
bool Camera::Convert2Mat(MV_FRAME_OUT_INFO_EX* pstImageInfo, unsigned char * pData)
{
    cv::Mat srcImage;
    if ( pstImageInfo->enPixelType == PixelType_Gvsp_Mono8 )
    {
        srcImage = cv::Mat(pstImageInfo->nHeight, pstImageInfo->nWidth, CV_8UC1, pData);
    }
    else if ( pstImageInfo->enPixelType == PixelType_Gvsp_RGB8_Packed )
    {
        RGB2BGR(pData, pstImageInfo->nWidth, pstImageInfo->nHeight);
        srcImage = cv::Mat(pstImageInfo->nHeight, pstImageInfo->nWidth, CV_8UC3, pData);
    }
    else
    {
        printf("unsupported pixel format\n");
        return false;
    }

    if ( NULL == srcImage.data )
    {
        return false;
    }

    //save converted image in a local file
    try {
#if defined (VC9_COMPILE)
        cvSaveImage("MatImage.bmp", &(IplImage(srcImage)));
#else
        //cv::imwrite("MatImage.bmp", srcImage);
        current_Image = srcImage.clone();
#endif
    }
    catch (cv::Exception& ex) {
        fprintf(stderr, "Exception saving image to bmp format: %s\n", ex.what());
    }

    srcImage.release();

    return true;
}


